//
// Created by Aron on 2024/10/31.
//

#include "Chassis.h"
#include "Speed_control.h"
#include "remote_control.h"
#include "CAN_receive.h"
pid_type_def pid_speed_Chassis_R;
pid_type_def pid_speed_Chassis_L;
static float set_speed_R;
static float set_speed_L;
static float n_C=5;



static int16_t max_speed_change_C=1000;
void Chassis_speedcontrol(void) {
    if(rc_ctrl.rc.ch[3]>=0) {
        if (rc_ctrl.rc.ch[2] <= 0) {
            set_speed_R = (rc_ctrl.rc.ch[3] - rc_ctrl.rc.ch[2]) * n_C;
            set_speed_L = rc_ctrl.rc.ch[3] * n_C;
        } else {
            set_speed_R = rc_ctrl.rc.ch[3] * n_C;
            set_speed_L = (rc_ctrl.rc.ch[3] + rc_ctrl.rc.ch[2]) * n_C;
        }
    }else{
        if (rc_ctrl.rc.ch[2] <= 0) {
            set_speed_R = (rc_ctrl.rc.ch[3] + rc_ctrl.rc.ch[2]) * n_C;
            set_speed_L = rc_ctrl.rc.ch[3] * n_C;
        } else {
            set_speed_R = rc_ctrl.rc.ch[3] * n_C;
            set_speed_L = (rc_ctrl.rc.ch[3] - rc_ctrl.rc.ch[2]) * n_C;
        }
    }
    Speed_out(set_speed_R,motor_measure[0].speed_rpm,max_speed_change_C,
              &pid_speed_Chassis_R);
    Speed_out(set_speed_L,motor_measure[1].speed_rpm,max_speed_change_C,
              &pid_speed_Chassis_L);
}
